#ifndef _ICP_2D_H_
#define _ICP_2D_H_ 

#include <iostream>
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>

//ros
#include <sensor_msgs/LaserScan.h>

#include <sophus/se3.hpp>
#include <sophus/se2.hpp>

class Icp2D
{
public:
    Icp2D();

    void setSrcCloud(const sensor_msgs::LaserScanConstPtr& scan);
    void setDstCloud(const sensor_msgs::LaserScanConstPtr& scan);

    void align(Sophus::SE2d& T_est);

    void alignWithLine(Sophus::SE2d& T_est);

private:
    void buildKdTree();

private:
    sensor_msgs::LaserScan::Ptr src_scan_;
    sensor_msgs::LaserScan::Ptr dst_scan_;

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
    
    Sophus::SE2d T_;
};



#endif